function [p,gp] = gpSARSA2(p,gp,ox,action,reward,x,next_action,n,t)
% [p,gp] = gpSARSA2(p,gp,ox,action,reward,x,next_action,n,t)
%
% gpSARSA(model,gp,xtmin1,xt,rtmin1,utmin1,ut,t)
% xtmin1: state at t-1
% xt: state at t
%
% GPDARSA implementation based on Y. Engel's 2005 dissertation (sparse
% recursive nonparameteric Monte-Carlo GPSARSA algo).
%
% Note: all timeseries start with t==0 in the first entry,
% i.e. var(1) is the value of var at time 0.
%
% Tobias Siegfried, 06/10/2008

%% init - ok
gp.okt = gp.kt;
gp.oa = gp.a;
gp.oc = gp.c;
gp.os = gp.s;
gp.od = gp.d;
coef = p.gamma(n) * gp.sigma2 / gp.os;
%% start
ktt = kernel(p,x,x,next_action,next_action);
gp.kt  = k(p,gp,x,next_action);
gp.a = gp.Kinv * gp.kt;
delta = ktt - gp.a' * gp.kt;
dk = gp.okt - p.gamma(n) * gp.kt;
gp.d = coef * gp.od + dk' * gp.alpha - reward;

if delta - p.sparseP > 0
    %disp(['add to dict for agent ' num2str(n) ' at time ' num2str(t) ' with delta=' num2str(delta)]);
    gp.h = [gp.oa; -p.gamma(n)];
    dktt = gp.oa' * (gp.okt - 2 * p.gamma(n) * gp.kt) + p.gamma(n)^2 * ktt;
    gp.c = coef * gp.oc + gp.C * dk - gp.oa;
    gp.s = (1 + p.gamma(n)^2) * gp.sigma2 + dktt - p.gamma(n)*gp.sigma2*coef - dk' *(gp.c+coef*gp.oc+gp.oa);
    gp.c = [gp.c; p.gamma(n)];
    gp.alpha = [gp.alpha; 0];
    first = [gp.C; zeros(1,size(gp.C,2))];
    gp.C = [first zeros(size(first,1),1)];
    gp.dict = updateD(gp,x,next_action);
    gp.Kinv = updateKInv(gp,delta);
    gp.a = [gp.z; 1];
    gp.z = [gp.z; 0];
    gp.kt = [gp.kt; ktt];  
    
else %if or(delta - p.sparseP < 0, t==p.tEnd)
    %disp(['no dict change for agent ' num2str(n) ' at time ' num2str(t)]);
    gp.h = gp.oa - p.gamma(n) * gp.a;
    % dktt = gp.h' * dk; % here again, inconsistent math! following Y.E. latest paper on the algo where dktt is in the update of gp.s.
    gp.c = coef * gp.oc + gp.C * dk - gp.h;
    %gp.s = (1 + p.gamma(n)^2) * gp.sigma2 + dktt - p.gamma(n) * gp.sigma2 * coef - dk' * (gp.c + coef * gp.oc);
    gp.s = (1 + p.gamma(n)^2) * gp.sigma2 - p.gamma(n) * gp.sigma2 * coef - dk' * (gp.c + coef * gp.oc);
end

gp.alpha = gp.alpha + gp.c * gp.d / gp.s;
gp.C = gp.C + 1 / gp.s * gp.c * gp.c';

function KInv = updateKInv(gp,delta)
% update KInv
aHat = gp.Kinv * gp.kt;
dkaa = delta * gp.Kinv + aHat * aHat';
first = [dkaa; -aHat'];
extend_aHat = [-aHat; 1];
KInv = 1 / delta * [first extend_aHat];

function res = k(p,gp,s,a)
% computes the sensation projected onto the kernelized space spanned by ...
% the sensations up until now, \mathbf{k}_t(x) = (k_(x_1,x),...,k(x_t,x))'
%res = kernel(p,[gp.dict(1:p.stateVDim,:) s],s,[gp.dict(p.stateVDim+1:end,:) a],a);
ds = gp.dict(1:p.stateVDim,:);
da = gp.dict(p.stateVDim+1:end,:);

res = kernel(p,ds,s,da,a);










